/*
 * @Author: zwf 240970521@qq.com
 * @Date: 2023-07-14 22:43:14
 * @LastEditors: zwf 240970521@qq.com
 * @LastEditTime: 2023-08-22 21:28:49
 * @FilePath: /esp32_cmake_test/motor/motor.c
 * @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
 */
#include "motor.h"
#include "delay.h"
#include "fluxObserver.h"
#include "math_common.h"
#include "motor_common.h"
#include <math.h>
#include <stddef.h>
#include "esp_log.h"
#include "modbusSlave.h"
#include "mbcontroller.h"
#include "motor_priv.h"
#include "smbObserver.h"

#define MOTOR_PWM_FREQ      30000
#define MOTOR_DUTY_RES      LEDC_TIMER_8_BIT
#define MOTOR_TASK_SIZE     1024 * 4
#define MOTOR_TASK_PRIORITY 10
#define MOTOR_ALIGN_UQ      0.6F

static char *tag = "motor";

static void  MOTOR_modbusMessageHandle(MotorDev *dev, mb_param_info_t regInfo)
{
    MotorObj     *priv      = dev->priv;
    MotorRegArea *regArea   = &priv->modbusReg;
    int           startAddr = regArea->startAddr;
    int           index     = regInfo.mb_offset - startAddr;

    ESP_LOGI(tag, "addr:%d, size:%d, time:%d\n", regInfo.mb_offset, regInfo.size, regInfo.time_stamp);
    /*这里的regInfo.size为读取的寄存器个数*/
    for (size_t i = index; i < index + regInfo.size; i += 2)
    {
        /*只对可写寄存器做出响应*/
        printf("%d\n", i + startAddr);
        switch (i)
        {
            case offsetof(MotorReg, setSpeed) / 2:
            {
                dev->setSpeed(dev, _IQ(regArea->reg.setSpeed));
                break;
            }

            case offsetof(MotorReg, setAngle) / 2:
            {
                /*待实现*/
                priv->setAngle = _IQ(regArea->reg.setAngle);
                break;
            }

            case offsetof(MotorReg, uq) / 2:
            {
                dev->setUq(dev, _IQ(regArea->reg.uq));
                break;
            }

            case offsetof(MotorReg, ud) / 2:
            {
                dev->setUd(dev, _IQ(regArea->reg.ud));
                break;
            }

            case offsetof(MotorReg, speedP) / 2:
            case offsetof(MotorReg, speedI) / 2:
            case offsetof(MotorReg, speedD) / 2:
            {
                priv->speedPID->setPID(priv->speedPID, _IQ(regArea->reg.speedP), _IQ(regArea->reg.speedI),
                                       _IQ(regArea->reg.speedD));
                break;
            }

            case offsetof(MotorReg, iqP) / 2:
            case offsetof(MotorReg, iqI) / 2:
            case offsetof(MotorReg, iqD) / 2:
            {
                priv->iqPID->setPID(priv->iqPID, _IQ(regArea->reg.iqP), _IQ(regArea->reg.iqI), _IQ(regArea->reg.iqD));
                break;
            }

            case offsetof(MotorReg, idP) / 2:
            case offsetof(MotorReg, idI) / 2:
            case offsetof(MotorReg, idD) / 2:
            {
                priv->idPID->setPID(priv->idPID, _IQ(regArea->reg.idP), _IQ(regArea->reg.idI), _IQ(regArea->reg.idD));
                break;
            }

            default:
            {
                ESP_LOGW(tag, "the reg[%ld] is unable write\n", i + startAddr);
                break;
            }
        }
    }
}

static void MOTOR_task(void *para)
{
    MotorDev       *dev  = para;
    MotorObj       *priv = dev->priv;
    mb_param_info_t regInfo;

    while (1)
    {
        if (pdTRUE == xQueueReceive(priv->modbusQueue, &regInfo, 0))
        {
            /*消息处理*/
            MOTOR_modbusMessageHandle(dev, regInfo);
        }

        // MOTOR_speedOpenCycle(dev);
#ifndef MOTOR_TIMER
        MOTOR_closeLoop(dev);
#endif
        // MOTOR_locationUpdate(dev);
#ifdef MOTOR_TIMER
        mdelay(5);
#else
        mdelay(1);
#endif
        // i++;
        // if (i % 200 == 0)
        // {
        //     bool a = esp_timer_is_active(priv->timerHandle);
        //     printf("motor timer is %d\n", a);
        //     esp_timer_dump(stdout);
        // }
    }
}

/*启动电机*/
static void MOTOR_start(MotorDev *dev)
{
    MotorObj *priv = NULL;

    if (!dev || !dev->priv)
    {
        return;
    }

    priv = dev->priv;
    vTaskResume(priv->task);
#ifdef MOTOR_TIMER
    esp_timer_start_periodic(priv->motorTimer, MOTOR_CYCLE);
    // gptimer_alarm_config_t alarmConfig = {
    //     .reload_count               = 0,
    //     .alarm_count                = 200,// period = 1ms
    //     .flags.auto_reload_on_alarm = true,
    // };
    // gptimer_set_alarm_action(priv->motorTimer, &alarmConfig);
    // gptimer_start(priv->motorTimer);
#endif
}

/*停止电机*/
static void MOTOR_stop(MotorDev *dev)
{
    MotorObj *priv = NULL;

    if (!dev || !dev->priv)
    {
        return;
    }

    priv = dev->priv;
    /*暂停任务前先停止电压输出*/
    for (int i = 0; i < MOTOR_PHASE_NUM; i++)
    {
        priv->phase[i]->setDuty(priv->phase[i], 0);
    }
    vTaskSuspend(priv->task);
    // gptimer_stop(priv->motorTimer);
}

/*设置电机ud参数*/
static void MOTOR_setUd(MotorDev *dev, _iq24 ud)
{
    MotorObj *priv = NULL;

    if (!dev || !dev->priv)
    {
        return;
    }

    priv = dev->priv;
    /*待加锁保护*/
    priv->voltage.d = ud;
    /*modbus寄存器赋值*/
    // ESP_LOGI(tag, "set ud:%f", priv->ud);
}

/*设置电机uq参数*/
static void MOTOR_setUq(MotorDev *dev, _iq24 uq)
{
    MotorObj *priv = NULL;

    if (!dev || !dev->priv)
    {
        return;
    }

    priv = dev->priv;

    // /*目前先把uq限制在电压的1/2*/
    // if (uq > priv->vMax / sqrtf(MOTOR_PHASE_NUM))
    // {
    //     return;
    // }

    /*待加锁保护*/
    priv->voltage.q = uq;
    /*modbus寄存器赋值*/
    priv->modbusReg.reg.uq = _IQtoF(uq);
    /*数据采样记录*/
    // ESP_LOGI(tag, "set uq:%f", priv->uq);
}

/*设置电机速度*/
static void MOTOR_setSpeed(MotorDev *dev, _iq21 speed)
{
    MotorObj *priv = NULL;

    if (!dev || !dev->priv)
    {
        return;
    }

    priv = dev->priv;
    /*待加锁保护*/
    priv->setSpeed = speed;
    // ESP_LOGI(tag, "set speed:%f", priv->setSpeed);
}

void IRAM_ATTR MOTOR_locationUpdate(MotorDev *dev)
{
    MotorObj *priv = NULL;

    if (!dev || !dev->priv)
    {
        return;
    }

    priv = dev->priv;
    if (!priv->sensor)
    {
        /*没注册sensor就什么都不干*/
        return;
    }

    /*观测器*/
    FluxObserverPara para = {.iAlpha = priv->ampare.alpha,
                             .iBeta  = priv->ampare.beta,
                             .vAlpha = priv->voltage.alpha,
                             .vBeta  = priv->voltage.beta};
    FLUX_observer(para);//110us,低速会抖,但是比上个抖动的轻,高速效果还可以,77莫得问题,似乎比上个好?
    priv->curAngle.timeStamp = esp_timer_get_time();
    /*更新角度*/
    priv->curAngle.angle = fluxOb.angle;
    priv->realSpeed      = fluxOb.speedHz;
    // printf("%f,%f,%f\n", priv->modbusReg.reg.ia, priv->modbusReg.reg.ib, priv->modbusReg.reg.ic);
}


static void MOTOR_alignSensor(MotorDev *dev)
{
    MotorObj *priv = NULL;

    if (!dev || !dev->priv)
    {
        return;
    }
    priv = dev->priv;

    if (!priv->sensor)
    {
        ESP_LOGE(tag, "the sensor is not register, can not close loop\n");
        return;
    }

    /*寻找传感器方向，移动一个电角度周期*/
    dev->setUq(dev, _IQ(MOTOR_ALIGN_UQ));
    dev->setUd(dev, 0);
    _iq24 freq = _IQ(0.002);
    for (int i = 0; i <= 500; i++)
    {
        _iq24 qAngle = _3pi_2 + _IQmpy(_IQmpyI32(freq, i), _2pi);
        FastSinCos(ElectricalIQAngle(qAngle), &priv->curAngle.valueSinCos);
        MOTOR_locationUpdate(dev);
        ParkReversalTrans(&priv->voltage, priv->curAngle.valueSinCos);
        ClarkReversalTrans(&priv->voltage);
        priv->setPhaseVoltage(dev);
        mdelay(2);
    }
    MOTOR_locationUpdate(dev);
    _iq24 midAngle = (int16_t)priv->curAngle.angle;
    /*移动回来*/
    for (int i = 500; i >= 0; i--)
    {
        _iq24 qAngle = _3pi_2 + _IQmpy(_IQmpyI32(freq, i), _2pi);
        FastSinCos(ElectricalIQAngle(qAngle), &priv->curAngle.valueSinCos);
        MOTOR_locationUpdate(dev);
        ParkReversalTrans(&priv->voltage, priv->curAngle.valueSinCos);
        ClarkReversalTrans(&priv->voltage);
        priv->setPhaseVoltage(dev);
        mdelay(2);
    }
    MOTOR_locationUpdate(dev);
    _iq24 endAngle = (int16_t)priv->curAngle.angle;
    dev->setUq(dev, 0);
    dev->setUd(dev, 0);
    FastSinCos(ElectricalIQAngle(_zero), &priv->curAngle.valueSinCos);
    ParkReversalTrans(&priv->voltage, priv->curAngle.valueSinCos);
    ClarkReversalTrans(&priv->voltage);
    priv->setPhaseVoltage(dev);
    mdelay(200);
    /*检测传感器移动方向*/
    _iq24 moved = _IQabs(midAngle - endAngle);
    if (moved < _min_angle_detect_movement)
    {
        priv->dir = 0;
    }
    else if (midAngle < endAngle)
    {
        priv->dir = -1;
    }
    else
    {
        priv->dir = 1;
    }
    ESP_LOGI(tag, "sensor dirtion is %d\n", priv->dir);

    /*检查电机极对数*/
    if (_IQabs(_IQmpyI32(moved, priv->pp) - _2pi) > _half)
    {
        // 0.5f is arbitrary number it can be lower or higher!
        ESP_LOGI(tag, "PP check: fail - estimated pp: %f\n", _2PI / moved);
    }
    else
    {
        ESP_LOGI(tag, "PP check: OK!");
    }

    SinCos sincos;
    _iq21  sumAngle = 0;
    /*校准0电角度*/
    dev->setUq(dev, _IQ(MOTOR_ALIGN_UQ));
    dev->setUd(dev, 0);
    for (int i = 0; i <= 500; i++)
    {
        FastSinCos(_3pi_2, &sincos);
        MOTOR_updateAmpere(dev);
        ClarkTrans(&priv->ampare);
        ParkTrans(&priv->ampare, sincos);
        MOTOR_locationUpdate(dev);
        ParkReversalTrans(&priv->voltage, sincos);
        ClarkReversalTrans(&priv->voltage);
        priv->setPhaseVoltage(dev);
        mdelay(2);
    }
    for (int i = 0; i <= 500; i++)
    {
        FastSinCos(_3pi_2, &sincos);
        MOTOR_updateAmpere(dev);
        ClarkTrans(&priv->ampare);
        ParkTrans(&priv->ampare, sincos);
        MOTOR_locationUpdate(dev);
        ParkReversalTrans(&priv->voltage, sincos);
        ClarkReversalTrans(&priv->voltage);
        priv->setPhaseVoltage(dev);
        mdelay(2);
        sumAngle += _IQtoIQ21(priv->curAngle.angle);
    }
    sumAngle = _IQ21div(sumAngle,_IQ21(500.0F));
    printf("angle = %f\n", _IQ21toF(sumAngle));
    priv->zeroElecAngle = ElectricalIQAngle(_IQ21toIQ(sumAngle));
    dev->setUq(dev, _IQ(0.01));
    dev->setUd(dev, 0);
    ParkReversalTrans(&priv->voltage, sincos);
    ClarkReversalTrans(&priv->voltage);
    priv->setPhaseVoltage(dev);
    ESP_LOGI(tag, "0电角度: %f\n", _IQtoF(priv->zeroElecAngle));
}

static void MOTOR_registerAmpereSensing(MotorDev *dev, CurrentSensing *sen)
{
    MotorObj *priv = NULL;

    if (!dev || !dev->priv || !sen)
    {
        return;
    }
    priv                = dev->priv;
    priv->ampereSensing = sen;
}

static void MOTOR_registerSensor(MotorDev *dev, AS5600Dev *sensor)
{
    MotorObj *priv = NULL;

    if (!dev || !dev->priv || !sensor)
    {
        return;
    }
    priv         = dev->priv;
    priv->sensor = sensor;
    /*获取当前传感器状态*/
    priv->curAngle.angle     = sensor->getAngle(sensor);
    priv->curAngle.timeStamp = esp_timer_get_time();
    priv->curAngle.nLaps     = 0;

    MOTOR_alignSensor(dev);
}

// /*获取电机速度*/
// void IRAM_ATTR MOTOR_getSpeed(MotorDev *dev)
// {
//     MotorObj        *priv   = dev->priv;
//     FilterAlgorithm *filter = priv->speedFilter;

//     // 速度计算

//     // printf("%f\n", priv->realSpeed);
//     // priv->realSpeed = lowPassFilter(priv->realSpeed);
//     /*滤波*/
//     priv->realSpeed             = filter->lowPassFilter(filter, priv->realSpeed);
//     priv->realSpeed             = filter->slideMeanFilter(filter, priv->realSpeed);

//     priv->prevAngle             = priv->curAngle;
//     priv->modbusReg.reg.focFreq = 1 / Ts;
// }


void IRAM_ATTR MOTOR_updateAmpere(MotorDev *dev)
{
    MotorObj       *priv = dev->priv;
    CurrentSensing *sen  = NULL;

    /*更新电流数据*/
    if (!priv->ampereSensing)
    {
        /*没注册电流采样，就不更新*/
        return;
    }

    sen = priv->ampereSensing;
    /*读取所有电流数据*/
    sen->readAll(sen);
    priv->ampare.a = sen->getIa(sen);
    priv->ampare.b = sen->getIb(sen);
    priv->ampare.c = sen->getIc(sen);
}


static void IRAM_ATTR MOTOR_setPhaseVoltage(MotorDev *dev)
{
    MotorObj *priv = dev->priv;
    _iq23     duty[MOTOR_PHASE_NUM];
    _iq24     max, min, ave, vOffset;

    if (!dev || !dev->priv)
    {
        return;
    }
    // int_fast64_t iqNResult;

    // iqNResult = (int_fast64_t)iqNInput1 * (int_fast64_t)iqNInput2;
    // iqNResult = iqNResult >> q_value;

    // return (int_fast32_t)iqNResult;
    /*svpwm*/
    max             = MAX(priv->voltage.a, priv->voltage.b);
    max             = MAX(max, priv->voltage.c);
    min             = MIN(priv->voltage.a, priv->voltage.b);
    min             = MIN(min, priv->voltage.c);
    ave             = _IQmpy(-(max + min), _half);

    priv->voltage.a += ave;
    priv->voltage.b += ave;
    priv->voltage.c += ave;

    vOffset         = _IQmpy(priv->vMax, _half);
    priv->voltage.a += vOffset;
    priv->voltage.b += vOffset;
    priv->voltage.c += vOffset;

    /*计算要设置的占空比*/
    duty[0] = _IQ24toIQ23(_IQsat(_IQmpy(priv->voltage.a, priv->_1_vMax), _one, _zero));
    duty[1] = _IQ24toIQ23(_IQsat(_IQmpy(priv->voltage.b, priv->_1_vMax), _one, _zero));
    duty[2] = _IQ24toIQ23(_IQsat(_IQmpy(priv->voltage.c, priv->_1_vMax), _one, _zero));

    /*设置占空比*/
    for (int i = 0; i < MOTOR_PHASE_NUM; i++)
    {
        duty[i] = _IQ23mpyI32(duty[i], priv->dutyCount);
        priv->phase[i]->setDuty(priv->phase[i], _IQ23int(duty[i]));
    }
}

/*电机删除函数*/
void MOTOR_delete(MotorDev *dev)
{
    MotorObj      *priv = NULL;
    ModbusSlaveReg reg;

    if (!dev)
    {
        return;
    }
    priv = dev->priv;

    if (priv)
    {
        if (priv->task)
        {
            /*停止任务*/
            dev->stop(dev);
            /*删除任务*/
            vTaskDelete(priv->task);
            /*删除modbus寄存器*/
            reg.regAddr = priv->modbusReg.startAddr;
            MODBUS_holdRegUnRegister(reg);
            /*删除消息队列*/
            vQueueDelete(priv->modbusQueue);
        }

        for (int i = 0; i < MOTOR_PHASE_NUM; i++)
        {
            if (priv->phase[i])
            {
                PWM_delete(priv->phase[i]);
            }
        }
        free(priv);
    }

    free(dev);
}

#ifdef MOTOR_TIMER
static void IRAM_ATTR MOTOR_timerHandle(void *arg)
{
    MotorDev *dev  = arg;
    MotorObj *priv = dev->priv;

    // MOTOR_closeLoop(dev);
    MOTOR_speedOpenCycle(dev);
    // esp_timer_start_once(priv->timerHandle, 1000);
}
#endif

static void MOTOR_constValueInit(MotorCfg cfg, MotorDev *dev)
{
    MotorObj *priv = dev->priv;

    /*初始化一些常量*/
    priv->vMax            = _IQ(cfg.vMax);
    priv->_1_vMax         = _IQdiv(_one, priv->vMax);
    priv->zeroElecAngle   = 0;
    priv->dutyCount       = (1 << MOTOR_DUTY_RES) - 1;
    priv->setPhaseVoltage = MOTOR_setPhaseVoltage;
    priv->pp              = cfg.pp;

    /*注册函数接口*/
    dev->start                 = MOTOR_start;
    dev->stop                  = MOTOR_stop;
    dev->setSpeed              = MOTOR_setSpeed;
    dev->setUd                 = MOTOR_setUd;
    dev->setUq                 = MOTOR_setUq;
    dev->registerSensor        = MOTOR_registerSensor;
    dev->registerAmpereSensing = MOTOR_registerAmpereSensing;
}

static esp_err_t MOTOR_createFilter(MotorDev *dev)
{
    MotorObj *priv = dev->priv;
    FilterCfg cfg;

    /*速度滤波*/
    cfg.lowPassAlpha = _IQ(0.005F);
    cfg.ts           = _IQ((float)MOTOR_CYCLE / 1e6);
    cfg.meanWinSize  = 3;
    if (FILTER_creat(&priv->speedFilter, cfg))
    {
        ESP_LOGE(tag, "motor speed filter fail\n");
        return ESP_FAIL;
    }

    /*iq滤波*/
    cfg.lowPassAlpha = _IQ(0.002F);
    cfg.meanWinSize  = 3;
    if (FILTER_creat(&priv->iqFilter, cfg))
    {
        ESP_LOGE(tag, "motor iq filter fail\n");
        return ESP_FAIL;
    }

    /*id滤波*/
    cfg.lowPassAlpha = _IQ(0.002F);
    cfg.meanWinSize  = 3;
    if (FILTER_creat(&priv->idFilter, cfg))
    {
        ESP_LOGE(tag, "motor id filter fail\n");
        return ESP_FAIL;
    }
    return ESP_OK;
}

static esp_err_t MOTOR_createPID(MotorDev *dev)
{
    MotorObj *priv = dev->priv;
    PidConfig pidCfg;
    MotorReg *reg = &priv->modbusReg.reg;

    /*初始化速度环,速度环下一级为电流环*/
    pidCfg.p          = _IQ(0.021);
    pidCfg.i          = _IQ(0.12F);
    pidCfg.d          = _IQ(0);
    pidCfg.outputRamp = _IQ20(1000);
    pidCfg.ts         = _IQ((float)MOTOR_CYCLE * 1e-6);
    pidCfg.limit      = _IQ(1); /*电流限制在400mA*/
    if (PID_create(&priv->speedPID, pidCfg))
    {
        ESP_LOGE(tag, "motor uq pid create error");
        return ESP_FAIL;
    }
    /*速度环modbus赋值*/
    reg->speedP = _IQtoF(pidCfg.p);
    reg->speedI = _IQtoF(pidCfg.i);
    reg->speedD = _IQtoF(pidCfg.d);

    /*初始化iq环*/
    pidCfg.p          = _IQ(0.025);
    pidCfg.i          = _IQ(0.25);
    pidCfg.d          = _IQ(0);
    pidCfg.outputRamp = _IQ21(0);
    pidCfg.limit      = _IQmpy(priv->vMax, _1_sqrt3);
    if (PID_create(&priv->iqPID, pidCfg))
    {
        ESP_LOGE(tag, "motor ud pid create error");
        return ESP_FAIL;
    }
    /*iq环modbus赋值*/
    reg->iqP = _IQtoF(pidCfg.p);
    reg->iqI = _IQtoF(pidCfg.i);
    reg->iqD = _IQtoF(pidCfg.d);

    /*初始化id环*/
    pidCfg.p          = _IQ(0.025);
    pidCfg.i          = _IQ(0.25);
    pidCfg.d          = _IQ(0);
    pidCfg.outputRamp = _IQ21(0);
    pidCfg.limit      = _IQmpy(priv->vMax, _1_sqrt3);
    if (PID_create(&priv->idPID, pidCfg))
    {
        ESP_LOGE(tag, "motor iq pid create error");
        return ESP_FAIL;
    }
    /*iq环modbus赋值*/
    reg->idP = _IQtoF(pidCfg.p);
    reg->idI = _IQtoF(pidCfg.i);
    reg->idD = _IQtoF(pidCfg.d);
    return ESP_OK;
}

/*电机创建*/
esp_err_t MOTOR_create(MotorCfg cfg, MotorDev **dev)
{
    MotorObj      *priv  = NULL;
    MotorDev      *motor = NULL;
    PwmCfg         pwmCfg;
    char           name[64];
    ModbusSlaveReg reg;

    if (!dev)
    {
        return ESP_FAIL;
    }

    motor = calloc(1, sizeof(MotorDev));
    if (!motor)
    {
        ESP_LOGE(tag, "calloc motor fail\n");
        return ESP_FAIL;
    }

    priv = calloc(1, sizeof(MotorObj));
    if (!motor)
    {
        ESP_LOGE(tag, "calloc motor priv fail\n");
        goto free;
    }

    /*数据赋值*/
    motor->priv = priv;

    /*配置电机pwm*/
    pwmCfg.dutyResolution = MOTOR_DUTY_RES;
    pwmCfg.timer          = cfg.timer;
    pwmCfg.freq           = MOTOR_PWM_FREQ;
    pwmCfg.speedMode      = LEDC_HIGH_SPEED_MODE;

    /*申请pwm对象*/
    for (int i = 0; i < MOTOR_PHASE_NUM; i++)
    {
        pwmCfg.pin = cfg.pin[i];
        if (PWM_create(pwmCfg, &priv->phase[i]))
        {
            ESP_LOGE(tag, "motor create pwm[%d] pin[%d] fail\n", i, pwmCfg.pin);
            goto free;
        }
    }

    MOTOR_constValueInit(cfg, motor);
    if (MOTOR_createPID(motor))
    {
        goto free;
    }

    /*创建modbus消息队列,用于响应主机的消息*/
    priv->modbusQueue = xQueueCreate(5, sizeof(mb_param_info_t));
    /*注册modbus寄存器*/
    priv->modbusReg.startAddr = cfg.modbusStartAddr;
    reg.queue                 = priv->modbusQueue;
    reg.addr                  = &priv->modbusReg.reg;
    reg.regAddr               = priv->modbusReg.startAddr;
    reg.size                  = sizeof(priv->modbusReg.reg);
    MODBUS_holdRegRegister(reg);

    /*创建低通滤波器*/
    if (MOTOR_createFilter(motor))
    {
        ESP_LOGE(tag, "motor filter fail\n");
        goto free;
    }

    /*创建任务*/
    sprintf(name, "motor[0x%x]", (int)motor);
    if (pdPASS != xTaskCreate(MOTOR_task, name, MOTOR_TASK_SIZE, motor, MOTOR_TASK_PRIORITY, &priv->task))
    {
        ESP_LOGE(tag, "motor task create fail\n");
        goto free;
    }
    /*先挂起任务*/
    vTaskSuspend(priv->task);

#ifdef MOTOR_TIMER
    /*创建定时器*/
    const esp_timer_create_args_t periodic_timer_args = {
        .callback        = MOTOR_timerHandle,
        .arg             = motor,
        .dispatch_method = ESP_TIMER_ISR,
        /* name is optional, but may help identify the timer when debugging */
        .name = "periodic"};
    esp_timer_create(&periodic_timer_args, &priv->motorTimer);

#endif

    /*输出电机对象*/
    *dev = motor;
    return ESP_OK;

free:
    MOTOR_delete(motor);
    return ESP_FAIL;
}
